/***************************************************************************
 创建者: 华磊
 开始时间: 2019.5.4
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/

#ifndef COUPLECONTROLMANAGEMENT_H
#define COUPLECONTROLMANAGEMENT_H
#include <QString>
#include <QVector>
#include <QMutex>
#include "globalDefine.h"
#include "../HmiGsoapQt/gSoapFoundation/soapStub.h"

//enum E_COUPLE_TYPE
//{
//    E_COUPLE_TYPE_COMMON,
//    E_COUPLE_TYPE_MAP_R,
//};

//struct CoupleConfigParameter
//{
//    volatile bool isValid;//是否工作
//    E_COUPLE_TYPE coupleType;
//    int driveAxisId;
//    int followAxisId;
//    volatile double followOffsetValue;
//    double allowMaxFollowOffset;
//    double coupleRatio;
//};

class AxisGroup;
class MotionMessage;
class FilteringAlgorithm;

class CoupleControlManagement
{
public:
    CoupleControlManagement(QString configFileIn, MotionMessage *motionMessageIn, int robotIdIn, AxisGroup *axisGroupIn);
public:
    bool isRelativeDistanceApproachTrigged();
    int setCoupleRelativeDistanceMonitorEnable(int followAxisId,bool isEnableIn);
    int getCoupleControlInfo(std::vector<struct CoupleConfigParameter> &infoOut);
    bool isCoupleControlFollowAxisWork(int followAxisId);
    int getCoupleConfigInfo(int followAxisId,CoupleConfigParameter &coupleParaOut);
    bool isInitialOk();
    bool isUseThisModule();
    int loopRealtimeOnce();
    int disableCouple();
    int setCoupleControlEnable(int followAxisId,bool isEnableIn);//注意线程安全问题，没有锁
    int setCoupleControlOffset(int followAxisId,double offfsetValueIn);//注意线程安全问题，没有锁
    int setAgvAngleInMap(double angleIn);//unit is degree
    int resetCoupleOffset();
    int disableAllCouple();

private:
    int translatePN180(double &degree);//聿冠ｂｕｇ,可能此处导致
    int resetCoupleOffset(CoupleConfigParameter &couplePara, AxisGroup *axisGroupIn);
    /**
     * @brief getBestPositionFromAngle
     * @param idealAngle -180~180
     * @param initialPosition 包含多圈的绝对角度
     * @param bestPositionOut　包含多圈的绝对角度
     * @return
     */
    int getBestPositionFromAngle(double idealAngle,double initialPosition,double &bestPositionOut);//not used
    int calculateOneCouple(const CoupleConfigParameter &couplePara,AxisGroup *axisGroupIn);
    int monitorRelativeDistance_OneCouple(CoupleConfigParameter &couplePara, AxisGroup *axisGroupIn);
    int initialConfigFile(QString configFileName);
    double getMinAngleReturn(double followValue,const CoupleConfigParameter &couplePara);
    double getMinAngleReturn_new(double followValue,const CoupleConfigParameter &couplePara);//是个ｂｕｇ ,聿冠测试，需要改进。超过圈数后可能就不对了。
    int checkFollowOffset(double oldPos, double newPos);
    void addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode
                , int robotIdIn, int parameter1=0, int parameter2=0, double parameter3=0,
                double parameter4=0, std::string messageInfo="");


private:
    bool isInitialOkStatus;
    bool isUseCoupleControl;
    MotionMessage *motionMessage;
    int robotId;
    AxisGroup *axisGroup;
    QVector<CoupleConfigParameter> coupleParameterList;
    double agvAngle_map;//agv在地图中的旋转角度，unit degree;
    FilteringAlgorithm* filtering_mapFollowAxisPos;
//    QMutex usingMutex;//有trajMutex锁保护了,可以不用该锁.
};

#endif // COUPLECONTROLMANAGEMENT_H
